The goals / steps of this project are the following:
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import math
%matplotlib qt
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')
N = len(images)
print(images)
print(type(images))
print("Number of images ==>", N)
# Step through the list and search for chessboard corners
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (9,6), corners, ret)
cv2.imshow('img',img)
cv2.waitKey(1000)
else:
print("No corners found in ", fname)
cv2.destroyAllWindows()
## Camera calibration given
%matplotlib inline
def cal_undistort(img, objpoints, imgpoints):
# Use cv2.calibrateCamera() and cv2.undistort()
cam_calib = cv2.calibrateCamera(objpoints, imgpoints, img.shape[:-1], None, None)
undist = cv2.undistort(img, cam_calib[1], cam_calib[2])
return undist
def PlotComparisonView(image_before, image_after, performedProcessing, colormap = (None, None)):
"""
To avoid repeting ploting commands every time, will be useful throughout
this notebook
"""
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image_before, cmap = colormap[0])
ax1.set_title('Original Image', fontsize=35)
ax2.imshow(image_after, cmap = colormap[1])
ax2.set_title(performedProcessing + ' Image', fontsize=35)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
plt.show()
img = cv2.imread(images[1])
print("objpoints array len", len( objpoints))
print("imgpoints array len", len( imgpoints))
undistorted_img = cal_undistort(img, objpoints, imgpoints)
PlotComparisonView(img, undistorted_img, 'Undistorted')
# Visualizing before and after of Undistortion on some chessboard images
for i in range(13,16):
img = cv2.imread(images[i])
undistorted_img = cal_undistort(img, objpoints, imgpoints)
PlotComparisonView(img, undistorted_img, 'Undistorted')
import matplotlib.image as mpimg
# Visualizing before and after of Undistortion on real world application image
test_images = glob.glob('test_images/test*.jpg')
for i in range(1,4):
img = mpimg.imread(test_images[i])
undistorted_img = cal_undistort(img, objpoints, imgpoints)
PlotComparisonView(img, undistorted_img, 'Undistorted Road')
def axis_gradient(img, ksize=9, thresh=(30,255)):
## x gradient
# convert to gray scale
#if channel == 'gray':
# gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
#else:
# gray_image = img
if len(img.shape) == 3:
gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
elif len(img.shape) ==2:
gray_image = img
height = img.shape[0]
width = img.shape[1]
# derivative along x
sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)
abs_sobel = np.absolute(sobelx)
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
single_axis_binary = np.zeros_like(gray_image, dtype=np.uint8)
#single_axis_binary = np.zeros((height,width,1), np.uint8)
single_axis_binary[ (scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
return single_axis_binary#cv2.Canny(gray_image, 20, 50) #
def magnitude_gradient(img, ksize=9, thresh=(50,255),channel='gray'):
## mag gradient
# convert to gray scale
#if channel == 'gray':
# gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
#else:
# gray_image = img
if len(img.shape) == 3:
gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
elif len(img.shape) ==2:
gray_image = img
# derivative along x and y
sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)
sobely = cv2.Sobel(gray_image, cv2.CV_64F, 0, 1, ksize)
sobelxy = np.sqrt(sobelx**2 + sobely**2)
scaled_sobel = np.uint8(255*sobelxy/np.max(sobelxy))
mag_binary = np.zeros_like(gray_image)
mag_binary[ (scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
return mag_binary
def direction_gradient(img, ksize=9, thresh=(0.7,1.3),channel='gray'):
## direction gradient
# convert to gray scale
#if channel == 'gray':
# gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
#else:
# gray_image = img
if len(img.shape) == 3:
gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
elif len(img.shape) ==2:
gray_image = img
# derivative along x and y
sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)
sobely = cv2.Sobel(gray_image, cv2.CV_64F, 0, 1, ksize)
abs_sobelx = np.absolute(sobelx)
abs_sobely = np.absolute(sobely)
sobel_direction = np.arctan2(abs_sobely, abs_sobelx)
dir_binary = np.zeros_like(sobel_direction)
dir_binary[ (sobel_direction >= thresh[0]) & (sobel_direction <= thresh[1])] = 1
return dir_binary
image = mpimg.imread(test_images[1])
# Apply all three distinct gradient
axis_binary_output = axis_gradient(image)
mag_binary_output = magnitude_gradient(image)
dir_binary_output = direction_gradient(image)
PlotComparisonView(image, axis_binary_output, 'Thresholded x-axis binary', (None, 'gray'))
PlotComparisonView(image, mag_binary_output, 'Thresholded Magnitude binary', (None, 'gray'))
PlotComparisonView(image, dir_binary_output, 'Thresholded Direction binary', (None, 'gray'))
# Play with threshold combination
combined_binary = np.zeros_like(axis_binary_output)
combined_binary[ (axis_binary_output==1) | (mag_binary_output*dir_binary_output == 1) ] = 1
PlotComparisonView(image, combined_binary, 'Combined Threshold binary', (None, 'gray'))
def combinedGradient_thresholdedBinary(img):
#print("Image shape ==>",img.shape)
axis_binary_output = axis_gradient(img)
mag_binary_output = magnitude_gradient(img)
dir_binary_output = direction_gradient(img)
combined_binary = np.zeros_like(axis_binary_output)
combined_binary[ (axis_binary_output==1) | (mag_binary_output*dir_binary_output == 1) ] = 1
return combined_binary
# Check on all the test images
for i in range ( len(test_images) ):
img = mpimg.imread(test_images[i])
combined_threshold = combinedGradient_thresholdedBinary(img)
PlotComparisonView(img, combined_threshold, 'Combined Thresholds ', (None, 'gray'))
#cv2.imwrite("output_images/write_test"+str(i)+".jpg", combined_threshold)
def to_HLS_Color_Channel(img, L_thresh=(200,255), S_thresh=(200,255)):
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
H_thresh = (10,40)
H = hls[:,:,0]
L = hls[:,:,1]
S = hls[:,:,2]
S_binary = np.zeros_like(S)
L_binary = np.zeros_like(L)
H_binary = np.zeros_like(H)
# S_binary[ ( ( L >= L_thresh[0] ) & ( L <= L_thresh[1] ) )
# | ( ( S >= S_thresh[0]) & (S <= S_thresh[1] ) ) ] = 1
L_binary[ (L >= L_thresh[0]) & (L <= L_thresh[1]) ] = 1 # to extract white lanes
S_binary[ (S >= S_thresh[0]) & (S <= S_thresh[1]) ] = 1
H_binary[ (H >= H_thresh[0]) & (H <= H_thresh[1]) ] = 1
return L_binary | S_binary
image = mpimg.imread(test_images[0])
#s_binary = to_S_Color_Channel(image,(190,255))
s_binary = to_HLS_Color_Channel(image)
PlotComparisonView(image, s_binary, 'HLS Channel ', (None, 'gray'))
# Check the color transform and the combined sobel gradients on all the test images
for i in range ( len(test_images) ):
img = mpimg.imread(test_images[i])
s_binary = to_HLS_Color_Channel(img)
combined_threshold = combinedGradient_thresholdedBinary(s_binary)
PlotComparisonView(img, combined_threshold, 'Combined Thresholds ', (None, 'gray'))
def drawConvexPolygonEdgesVertices(img, pts, edge = True, vertex = True):
"""
pts: a sequence of pts that create a convex polygon
"""
col = [255, 0, 0]
thick = 3
rad = 15
img_copy = img.copy() # avoid img_copy = img as both images will be modified
if vertex == True:
# draw vertices
for pt in pts:
cv2.circle(img_copy, pt, rad, col, -thick, lineType=8, shift=0)
if edge == True:
# draw edges
N = len(pts)
for i in range(N):
cv2.line(img_copy, pts[ i ], pts[ (i+1) % N ], col, thick)
return img_copy
%matplotlib qt
image = mpimg.imread(test_images[2])
image = cal_undistort(image, objpoints, imgpoints)
img_size = ( image.shape[1], image.shape[0] )
# Manually selecting the cordinnates for a src points of RoI
src_p1 = (316,660) # bottom left
src_p2 = (1050,660) # bottom right
src_p3 = (762,480) # top right
src_p4 = (578,480) # top left
src = np.float32([ src_p1, src_p2, src_p3, src_p4 ])
image_RoI = drawConvexPolygonEdgesVertices(image, [ src_p1, src_p2, src_p3, src_p4 ])
plt.imshow(image_RoI)
plt.show()
%matplotlib inline
height = image.shape[0]
width = image.shape[1]
# Manually selecting the cordinnates for a dst points of RoI
# Might need a more reliable way to select src and dst points
dst_p1 = (300, height - 30) # bottom left
dst_p2 = (width-330, height - 30) # bottom right
dst_p3 = (width - 250, 250) # top right
dst_p4 = (300+70, 250) # top left
dst = np.float32([ dst_p1, dst_p2, dst_p3, dst_p4])
# generate perspective transofrmation matrix given source and destination points
M = cv2.getPerspectiveTransform(src, dst)
# perform perspective transformation
warped = cv2.warpPerspective(image, M, img_size, flags=cv2.INTER_LINEAR)
warped_RoI = drawConvexPolygonEdgesVertices(warped, [ dst_p1, dst_p2, dst_p3, dst_p4])
PlotComparisonView(image_RoI, warped_RoI, 'Perspective transformed ', colormap = (None, None))
# Check result for all test images
for i in range ( len(test_images) ):
img = mpimg.imread(test_images[i])
undistort_img = cal_undistort(img, objpoints, imgpoints)
warped_img = cv2.warpPerspective(undistort_img, M, img_size, flags=cv2.INTER_LINEAR)
PlotComparisonView(undistort_img, warped_img, 'Perspective transformed ', (None, 'gray'))
# Check perspective transfom result for all test binary images
test_warped_imgs = []
for i in range ( len(test_images) ):
img = mpimg.imread(test_images[i])
undistort_img = cal_undistort(img, objpoints, imgpoints)
s_binary = to_HLS_Color_Channel(img)
combined_threshold = combinedGradient_thresholdedBinary(s_binary)
warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
test_warped_imgs += [warped_img]
PlotComparisonView(combined_threshold, warped_img, 'Perspective transformed ', ('gray', 'gray'))
# reuse histogram from course material
def hist(img):
# TO-DO: Grab only the bottom half of the image
# Lane lines are likely to be mostly vertical nearest to the car
height = img.shape[0]
bottom_half = img[ height//2:,: ]
# TO-DO: Sum across image pixels vertically - make sure to set `axis`
# i.e. the highest areas of vertical lines should be larger values
histogram = np.sum( bottom_half, axis=0)
return histogram
for i in range(6):
# Create histogram of image binary activations
histogram = hist(test_warped_imgs[i])
# Visualize the resulting histogram
fig = plt.figure(1, figsize=(10, 4))
plt.title("Random title")
plt.grid()
plt.plot(histogram)
plt.show()
def detectLanePixels(test_im, stats=True):
# HYPERPARAMETERS
# Choose the number of sliding windows
nwindows = 9
# Set the width of the windows +/- margin
margin = 100
# Set minimum number of pixels found to recenter window
minpix = 50
# Make sure image values are in [0, 255] interval
if np.max(test_im) == 1:
test_im = np.uint8(255*test_im)
# Create a histogram for the second half of the image
histogram = hist(test_im)
# Create output image to draw on and visualize result
out_img = np.dstack((test_im, test_im, test_im))
# peak of left and right part of the histogram are starting points of left and right lanes
midpoint = np.int(histogram.shape[0]//2)
leftxbase = np.argmax(histogram[:midpoint])
rightxbase = np.argmax(histogram[midpoint:]) + midpoint
# Set height of windows - based on nwindows above and image shape
window_height = np.int(test_im.shape[0]//nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = test_im.nonzero() # nonzero is a 2-elements tuple
nonzeroy = np.array(nonzero[0]) # first element is an array of y coordinates of non-zero pixels
nonzerox = np.array(nonzero[1]) # second element is an array of x coordinates of non-zero pixels
# Current positions to be updated later for each window in nwindows
leftx_current = leftxbase
rightx_current = rightxbase
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# This list is only for a general stat purpose
activatedPixels_perWindow = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = test_im.shape[0] - (window+1)*window_height
win_y_high = test_im.shape[0] - window*window_height
#Find the four boundaries of the left & right windows on x axis
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
#Identify the nonzero pixels in x and y within the window
left_nonzero_pixels = ( (nonzerox > win_xleft_low) & (nonzerox < win_xleft_high)
& (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]
right_nonzero_pixels = ( (nonzerox > win_xright_low) & (nonzerox < win_xright_high)
& (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]
#print(len(left_nonzero_pixels), len(right_nonzero_pixels)) # How many non zero pixels indices are on
# the left & right line in current window
activatedPixels_perWindow.insert(0,[len(left_nonzero_pixels), len(right_nonzero_pixels)])
# add the result to the one from previous windows
left_lane_inds.append(left_nonzero_pixels)
right_lane_inds.append(right_nonzero_pixels)
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low), (win_xleft_high,win_y_high),(0,255,0), 5)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 5)
# recalculating the base x-coordinates of left and right windows
if (len(left_nonzero_pixels) > minpix):
# Position of activated pixels in current left window
leftx_pixelpos_current = nonzerox[left_nonzero_pixels]
leftx_current = np.int(np.mean( leftx_pixelpos_current, axis=0) )
#print("new left base is ==>", leftx_current)
if ( len( right_nonzero_pixels ) > minpix):
# Position of activated pixels in current right window
rightx_pixelpos_current = nonzerox[right_nonzero_pixels]
rightx_current = np.int(np.mean( rightx_pixelpos_current, axis=0) )
#print("new right base is ==>", rightx_current)
# reorganise indices across all windows
left_lane_inds = np.concatenate( left_lane_inds )
right_lane_inds = np.concatenate( right_lane_inds )
if stats == True:
print("leftxbase : ", leftxbase, 'rightxbase : ', rightxbase)
print("Number of non zero pixels are in the whole image", nonzero[0].shape) #how many non zero pixels are in the whole image
print("Number of non zero pixels indices on the left lane", left_lane_inds.shape) # how many non zero pixels indices are on the left line
print("Number of non zero pixels indices on the right lane", right_lane_inds.shape) # how many non zero pixels indices are on the right line
print("Number of activated pixels per window", np.array(activatedPixels_perWindow) )
# get pixel position from pixel indices
leftx_pos = nonzerox[left_lane_inds]
lefty_pos = nonzeroy[left_lane_inds]
rightx_pos = nonzerox[right_lane_inds]
righty_pos = nonzeroy[right_lane_inds]
out_img[ lefty_pos, leftx_pos] = [255, 0, 0]
out_img[ righty_pos, rightx_pos] = [0, 0, 255]
return leftx_pos, lefty_pos, rightx_pos, righty_pos, out_img
# Apply on all test images
for i in range(5):
test_img = test_warped_imgs[i]
lane_pixel_img = detectLanePixels(test_img, stats=False)[4]
plt.imshow(lane_pixel_img)
plt.show()
def fit_polynomial_toLane(image, xm_ym_per_pix = (3.7/700, 30/720), plot_poly=True):
# fit a polynomial into the detected pixels so far
test_img = test_warped_imgs[5]
leftx, lefty, rightx, righty, lanePixel_im = detectLanePixels(image, stats=False)
# fitting into a 2nd order polynomial x = f(y)
#leftfit = np.polyfit( lefty*xm_ym_per_pix[1], leftx*xm_ym_per_pix[0], 2 )
#rightfit = np.polyfit( righty*xm_ym_per_pix[1], rightx*xm_ym_per_pix[0], 2)
leftfit = np.polyfit( lefty, leftx, 2 )
rightfit = np.polyfit( righty, rightx, 2)
#print("leftfit", leftfit, "rightfit", rightfit)
# Use the fitting parameters to plot the polynomial on the image
## generate y axis base
ploty = np.linspace(0, image.shape[0]-1, image.shape[0]).astype(int)
#ploty = ploty*xm_ym_per_pix[1]
## compute leftx = f_left(y) & rightx = f_right(y)
plot_leftx = ( leftfit[0]*ploty**2 + leftfit[1]*ploty + leftfit[2] ).astype(int)
plot_rightx = ( rightfit[0]*ploty**2 + rightfit[1]*ploty + rightfit[2] ).astype(int)
# plot left and right fitting on lanePixel_im
#if plot_poly == True:
#plt.plot( plot_leftx, ploty, color='yellow' )
#plt.plot( plot_rightx, ploty, color='yellow')
#print(ploty)
#print(plot_rightx)
for paint in range(-5,5):
try:
lanePixel_im[ ploty, plot_leftx+paint] = [255, 0, 0] # will be painting only a single pixel per y value => not very visible
lanePixel_im[ ploty, plot_rightx+paint] = [255, 0, 0]
except:
break
#return leftfit, rightfit, lanePixel_im
return leftfit, rightfit, lanePixel_im
# Let's run on all 6 images
for i in range(6):
current_test_img = test_warped_imgs[i]
current_out_img = fit_polynomial_toLane(current_test_img)[2]
plt.imshow(current_out_img)
#cv2.imwrite("output_images/polyfit"+str(i)+".jpg", current_out_img)
plt.show()
def compute_laneCurvature_vehiculeOffset(test_im):
# introducing the conversion from pixel world to real scale meter world
xm_per_pix = 3.7/700 # a rigourous reason should be given here?
ym_per_pix = 30/720
width = image.shape[1]
left_fit, right_fit, polyfit_im = fit_polynomial_toLane(test_im, (xm_per_pix,ym_per_pix), plot_poly=True)
plt.imshow(polyfit_im)
ploty = np.linspace(0, test_im.shape[0]-1, test_im.shape[0])
# Select the point where the curvature is calculated
y_eval = np.max(ploty)
a_l = left_fit[0]
b_l = left_fit[1]
a_r = right_fit[0]
b_r = right_fit[1]
# compute left and right curvature using formula from class material
left_curverad = np.power(1 + (2*left_fit[0]*y_eval+left_fit[1])**2, 3/2)/abs(2*left_fit[0])
right_curverad = np.power(1 + (2*right_fit[0]*y_eval+right_fit[1])**2, 3/2)/abs(2*right_fit[0])
print("left_lane_curvature", left_curverad, "right_lane_curvature", right_curverad)
########## Vehicle position ###########
## compute leftx = f_left(y) & rightx = f_right(y)
plot_leftx = ( left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2] ).astype(int)
plot_rightx = ( right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2] ).astype(int)
vehicle_pos = ( plot_rightx[-1] + plot_leftx[-1] )//2 # midpoint of the two lines base
vehicle_offset = xm_per_pix*( vehicle_pos - width//2 )
#print("Vehicle position -->", vehicle_pos)
print("Vehicle offset -->", vehicle_offset)
return left_curverad, right_curverad, vehicle_offset
test_img = test_warped_imgs[1]
print( compute_laneCurvature_vehiculeOffset( test_img ) )
# perspective transform inverse
# generate perspective transofrmation matrix given source and destination points
Minv = cv2.getPerspectiveTransform(dst, src)
warped_imgs = [] # inversed perspective images
for i in range (6):
test_img = test_warped_imgs[i]
lane_pixel_img = fit_polynomial_toLane(test_img)[2]
# perform perspective transformation
warped = cv2.warpPerspective(lane_pixel_img, Minv, img_size, flags=cv2.INTER_LINEAR)
warped_imgs += [warped]
PlotComparisonView(lane_pixel_img, warped, 'inverse perspective', colormap = (None, None))
#plt.imshow(warped)
#plt.show()
combined_imgs = []
for i in range(5):
warped = warped_imgs[i]
img = mpimg.imread(test_images[i])
combined_img = warped | img
combined_imgs += [combined_img]
PlotComparisonView(warped, combined_img, 'Final perspective', colormap = (None, None))
#cv2.imwrite("output_images/finalPerspective"+str(i)+".jpg", cv2.cvtColor(combined_img, cv2.COLOR_RGB2BGR))
def advancedLaneFindingPipeline(input_img):
Pipeline = []
# undistort input image
undistort_img = cal_undistort(input_img, objpoints, imgpoints)
Pipeline += [undistort_img]
# create a binary from HLS color space
hls_binary = to_HLS_Color_Channel(undistort_img)
Pipeline += [hls_binary]
# create a binary using axis, magnitude and direction gradient on hls_binary
combined_threshold = combinedGradient_thresholdedBinary(hls_binary)
Pipeline += [combined_threshold]
# perform perspective transform to bird-eye view
warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
Pipeline += [warped_img]
# detect lane pixel on bird-eye view image and generate fitting polynomial
lane_pixel_img = fit_polynomial_toLane(warped_img)[2]
Pipeline += [lane_pixel_img]
current_fit = fit_polynomial_toLane(warped_img)[:2]
print(current_fit)
# perform inverse perspective transformation from bird-eye view
unwarped_img = cv2.warpPerspective(lane_pixel_img, Minv, img_size, flags=cv2.INTER_LINEAR)
Pipeline += [unwarped_img]
# draw result on input_img
finalresult_img = unwarped_img | undistort_img
Pipeline += [finalresult_img]
#PlotComparisonView(input_img, finalresult_img, 'Final perspective', colormap = (None, None))
# save result into output_images folder
#cv2.imwrite("output_images/finalPerspective"+str(i)+".jpg", cv2.cvtColor(combined_img, cv2.COLOR_RGB2BGR))
# save result into output_images folder
#for i in range( len(Pipeline)):
# if 1<= i <=3:
# cv2.imwrite("output_images/SingleImgs"+str(i)+".jpg", 255*Pipeline[i])
# else:
# cv2.imwrite("output_images/SingleImgs"+str(i)+".jpg", cv2.cvtColor(Pipeline[i], cv2.COLOR_RGB2BGR))
return finalresult_img
for i in range(6):
my_img = mpimg.imread(test_images[i])
final = advancedLaneFindingPipeline(my_img)
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
def process_image(image):
# NOTE: The output you return should be a color image (3 channel) for processing video below
# TODO: put your pipeline here,
result = advancedLaneFindingPipeline(image)
# you should return the final output (image where lines are drawn on lanes)
return result
video_output = 'output_videos/project_video_notFinal.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
clip1 = VideoFileClip("project_video.mp4").subclip(0, 2)
print(type(clip1))
#clip1 = VideoFileClip("project_video.mp4")
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(video_output))
The main two points that will be adressed are:
# Define a class to receive the characteristics of each line detection
class Line():
def __init__(self):
# was the line detected in the last iteration?
self.detected = False
# x values of the last n fits of the line
self.recent_xfitted = []
#average x values of the fitted line over the last n iterations
self.bestx = None
#polynomial coefficients averaged over the last n iterations
self.best_fit = None
#polynomial coefficients for the most recent fit
self.current_fit = [np.array([False])]
#polynomial coefficients for the most recent fits
self.recent_fits = [np.array([False])]
#radius of curvature of the line in some units
self.radius_of_curvature = None
#radius of curvature Array of the line in some units
self.radius_of_curvature_array = [np.array([])]
#distance in meters of vehicle center from the line
self.line_base_pos = None
# tracking number of missed lines in a row
self.missCounter = 4 # initialized to 4 to be able to use sliding windows in the first run
# Create two instances from this class
left_line = Line()
right_line = Line()
sliding_windows that decides whether we want to perform a sliding windows search all over again in the full frame, or just search locally around the previous detections.def detectLanePixels_vid(test_im, sliding_windows, stats=True):
"""
This function returns the positions of activated pixels in left and right lanes
and the output image with those pixels painted
"""
# HYPERPARAMETERS
# Choose the number of sliding windows
nwindows = 9
# Set the width of the windows +/- margin
margin = 100
# Set minimum number of pixels found to recenter window
minpix = 50
# Make sure image values are in [0, 255] interval
if np.max(test_im) == 1:
test_im = np.uint8(255*test_im)
# Create a histogram for the second half of the image
histogram = hist(test_im)
# Create output image to draw on and visualize result
out_img = np.dstack((test_im, test_im, test_im))
# peak of left and right part of the histogram are starting points of left and right lanes
midpoint = np.int(histogram.shape[0]//2)
leftxbase = np.argmax(histogram[:midpoint])
rightxbase = np.argmax(histogram[midpoint:]) + midpoint
# Set height of windows - based on nwindows above and image shape
window_height = np.int(test_im.shape[0]//nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = test_im.nonzero() # nonzero is a 2-elements tuple
nonzeroy = np.array(nonzero[0]) # first element is an array of y coordinates of non-zero pixels
nonzerox = np.array(nonzero[1]) # second element is an array of x coordinates of non-zero pixels
# Current positions to be updated later for each window in nwindows
leftx_current = leftxbase
rightx_current = rightxbase
if sliding_windows == True: # use sliding windows approach
print("--> Using Sliding Windows Approach <--")
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# This list is only for a general stat purpose
activatedPixels_perWindow = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = test_im.shape[0] - (window+1)*window_height
win_y_high = test_im.shape[0] - window*window_height
#Find the four boundaries of the left & right windows on x axis
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
#Identify the nonzero pixels in x and y within the window
left_nonzero_pixels = ( (nonzerox > win_xleft_low) & (nonzerox < win_xleft_high)
& (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]
right_nonzero_pixels = ( (nonzerox > win_xright_low) & (nonzerox < win_xright_high)
& (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]
#print(len(left_nonzero_pixels), len(right_nonzero_pixels)) # How many non zero pixels indices are on
# the left & right line in current window
activatedPixels_perWindow.insert(0,[len(left_nonzero_pixels), len(right_nonzero_pixels)])
# add the result to the one from previous windows
left_lane_inds.append(left_nonzero_pixels)
right_lane_inds.append(right_nonzero_pixels)
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low), (win_xleft_high,win_y_high),(0,255,0), 5)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 5)
# recalculating the base x-coordinates of left and right windows
if (len(left_nonzero_pixels) > minpix):
# Position of activated pixels in current left window
leftx_pixelpos_current = nonzerox[left_nonzero_pixels]
leftx_current = np.int(np.mean( leftx_pixelpos_current, axis=0) )
#print("new left base is ==>", leftx_current)
if ( len( right_nonzero_pixels ) > minpix):
# Position of activated pixels in current right window
rightx_pixelpos_current = nonzerox[right_nonzero_pixels]
rightx_current = np.int(np.mean( rightx_pixelpos_current, axis=0) )
#print("new right base is ==>", rightx_current)
# reorganise indices across all windows
left_lane_inds = np.concatenate( left_lane_inds )
right_lane_inds = np.concatenate( right_lane_inds )
elif sliding_windows == False: # Use info from previous frames
print("--> Searching Around previous PolyFits Approach <--")
margin = 100
# Get left_fit and right_fit of last frame lines instances
last_left_fit = left_line.best_fit
last_right_fit = right_line.best_fit
# Create lists to receive left and right lane pixel indices by exploring the region next to where
# last lanes polyfit were found
left_lane_inds = ( (nonzerox > (last_left_fit[0]*(nonzeroy**2) + last_left_fit[1]*nonzeroy +
last_left_fit[2] - margin) ) & (nonzerox < (last_left_fit[0]*(nonzeroy**2) +
last_left_fit[1]*nonzeroy + last_left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (last_right_fit[0]*(nonzeroy**2) + last_right_fit[1]*nonzeroy +
last_right_fit[2] - margin)) & (nonzerox < (last_right_fit[0]*(nonzeroy**2) +
last_right_fit[1]*nonzeroy + last_right_fit[2] + margin)))
# get pixel position from pixel indices
leftx_pos = nonzerox[left_lane_inds]
lefty_pos = nonzeroy[left_lane_inds]
rightx_pos = nonzerox[right_lane_inds]
righty_pos = nonzeroy[right_lane_inds]
out_img[ lefty_pos, leftx_pos] = [255, 0, 0]
out_img[ righty_pos, rightx_pos] = [0, 0, 255]
if stats == True:
print("leftxbase : ", leftxbase, 'rightxbase : ', rightxbase)
print("Number of non zero pixels are in the whole image", nonzero[0].shape) #how many non zero pixels are in the whole image
print("Number of non zero pixels indices on the left lane", left_lane_inds.shape) # how many non zero pixels indices are on the left line
print("Number of non zero pixels indices on the right lane", right_lane_inds.shape) # how many non zero pixels indices are on the right line
return leftx_pos, lefty_pos, rightx_pos, righty_pos, out_img
def fit_polynomial_toLane_vid(image, last_fit, xm_ym_per_pix = (3.7/700, 30/720), plot_poly=True):
# fit a polynomial into the detected pixels so far
test_img = test_warped_imgs[5]
leftx, lefty, rightx, righty, lanePixel_im = detectLanePixels_vid(image, last_fit, stats=False)
# fitting into a 2nd order polynomial x = f(y)
#leftfit = np.polyfit( lefty*xm_ym_per_pix[1], leftx*xm_ym_per_pix[0], 2 )
#rightfit = np.polyfit( righty*xm_ym_per_pix[1], rightx*xm_ym_per_pix[0], 2)
leftfit = np.polyfit( lefty, leftx, 2 )
rightfit = np.polyfit( righty, rightx, 2)
#print("leftfit", leftfit, "rightfit", rightfit)
# Use the fitting parameters to plot the polynomial on the image
## generate y axis base
width = image.shape[1]
height = image.shape[0]
ploty = np.linspace(0, height-1, height).astype(int)
#ploty = ploty*xm_ym_per_pix[1]
## compute leftx = f_left(y) & rightx = f_right(y)
plot_leftx = ( leftfit[0]*ploty**2 + leftfit[1]*ploty + leftfit[2] ).astype(int)
plot_rightx = ( rightfit[0]*ploty**2 + rightfit[1]*ploty + rightfit[2] ).astype(int)
# plot left and right fitting on lanePixel_im
#if plot_poly == True:
#plt.plot( plot_leftx, ploty, color='yellow' )
#plt.plot( plot_rightx, ploty, color='yellow')
#print(ploty)
#print(plot_rightx)
for paint in range(-5,5):
try:
lanePixel_im[ ploty, plot_leftx+paint] = [255, 0, 0]
lanePixel_im[ ploty, plot_rightx+paint] = [255, 0, 0]
except:
break
######### Curvature radius ##########
## fit poly in real world space
y_eval = np.max(ploty)
xm_per_pix = xm_ym_per_pix[0]
ym_per_pix = xm_ym_per_pix[1]
leftfit_rw = np.polyfit( lefty*ym_per_pix, leftx*xm_per_pix, 2 )
rightfit_rw = np.polyfit( righty*ym_per_pix, rightx*xm_per_pix, 2)
# Calculation of R_curve (radius of curvature)
left_curverad = ((1 + (2*leftfit_rw[0]*y_eval*ym_per_pix + leftfit_rw[1])**2)**1.5) / np.absolute(2*leftfit_rw[0])
right_curverad = ((1 + (2*rightfit_rw[0]*y_eval*ym_per_pix + rightfit_rw[1])**2)**1.5) / np.absolute(2*rightfit_rw[0])
left_line.radius_of_curvature_array.append(left_curverad)
right_line.radius_of_curvature_array.append(right_curverad)
print("Curvature radius Real World - Left =>", left_curverad, "m - Right", right_curverad, "m")
########## Vehicle position ###########
vehicle_pos = ( plot_rightx[-1] + plot_leftx[-1] )//2 # midpoint of the two lines base
vehicle_offset = xm_per_pix*( vehicle_pos - width//2 )
#print("Vehicle position -->", vehicle_pos)
print("Vehicle offset -->", vehicle_offset)
########## Sanity Check ############
sanity = True
## Inconsistent distance between the two lanes across the heigth of the road
LinesDiff = plot_rightx - plot_leftx
NbPointsPerSegment = 30
top = sum( LinesDiff[ :NbPointsPerSegment ] )
middle = sum( LinesDiff[ height//2:height//2 + NbPointsPerSegment] )
bottom = sum( LinesDiff[ -NbPointsPerSegment:] )
print("Top --->", top, "Middle --->", middle, "Bottom --->", bottom )
curvrad_diff = 2000
if abs( top - middle ) > curvrad_diff or abs( top - bottom ) > curvrad_diff or abs( middle - bottom ) > curvrad_diff:
print( "---> Distance between lines is not consistent <---")
leftfit = left_line.best_fit
rightfit = right_line.best_fit
left_line.detected = False
right_line.detected = False
left_line.missCounter += 1
right_line.missCounter += 1
sanity = False
else:
# Sanity Check successful - current lines to be included
left_line.detected = True
right_line.detected = True
left_line.radius_of_curvature = int( left_curverad )
right_line.radius_of_curvature = int( right_curverad )
left_line.line_base_pos = vehicle_offset
left_line.recent_fits.append( leftfit )
right_line.recent_fits.append( rightfit )
## The approach of computing Average is subject to improvement
n = 3 # number of frames across which we want to compute the average
if len(left_line.recent_fits) > n and left_line.missCounter <= 3:
left_line.best_fit = np.average(left_line.recent_fits[-n:], axis=0)
right_line.best_fit = np.average(right_line.recent_fits[-n:], axis=0)
else:
# don't use the average
left_line.best_fit = leftfit
right_line.best_fit = rightfit
sanity = True
return left_line.best_fit, right_line.best_fit, lanePixel_im, sanity
def putTextToFrame(image, curvature, offset):
#### Write curvature radius and vehicule offset info to final image
# font
font = cv2.FONT_HERSHEY_SIMPLEX
# fontScale
fontScale = 1.5
# Blue color in BGR
color = (230, 0, 0)
# Line thickness of 2 px
thickness = 2
# Using cv2.putText() method
msg1 = 'Curvature Radius : ' + str(curvature)+' m'
msg2 = 'Vehicule Offset : ' + str( round(offset, 2) ) + 'm'
image = cv2.putText(image, msg1, (300, 100) , font, fontScale, color, thickness, cv2.LINE_AA)
image = cv2.putText(image, msg2, (300, 200), font, fontScale, color, thickness, cv2.LINE_AA)
return image
def advancedLaneFindingPipeline_vid(input_img):
Pipeline = []
# undistort input image
undistort_img = cal_undistort(input_img, objpoints, imgpoints)
Pipeline += [undistort_img]
# create a binary from HLS color space
hls_binary = to_HLS_Color_Channel(undistort_img)
Pipeline += [hls_binary]
# create a binary using axis, magnitude and direction gradient on hls_binary
combined_threshold = combinedGradient_thresholdedBinary(hls_binary)
Pipeline += [combined_threshold]
# perform perspective transform to bird-eye view
warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
Pipeline += [warped_img]
# Detect lane pixel on bird-eye view image and generate fitting polynomial
########### Decide between Sliding Windows or Local search ############
missThreshold = 8
# Reperform a sliding windows search if accumulated a number of misses above threshold
if left_line.missCounter + right_line.missCounter >= missThreshold:
sliding_windows = True
left_line.missCounter = 0
right_line.missCounter = 0
else:
sliding_windows = False
current_left_fit, current_right_fit, lane_pixel_img, sanity = fit_polynomial_toLane_vid(warped_img, sliding_windows, plot_poly=False)
Pipeline += [lane_pixel_img]
############ Drawing Fill Poly Region into input frame ################
## Used code from course material - note: might want to add it as a separate helper function for future use
# Create an image to draw the lines on
warp_zero = np.zeros_like(warped_img).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
# These can be added as attributes hence directly accessible ?
ploty = np.linspace(0, image.shape[0]-1, image.shape[0])
left_fitx = ( current_left_fit[0]*ploty**2 + current_left_fit[1]*ploty + current_left_fit[2] ).astype(int)
right_fitx = ( current_right_fit[0]*ploty**2 + current_right_fit[1]*ploty + current_right_fit[2] ).astype(int)
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0]))
# Combine the result with the original image
finalresult_img = cv2.addWeighted(input_img, 1, newwarp, 0.3, 0)
finalresult_img = putTextToFrame(finalresult_img, left_line.radius_of_curvature, left_line.line_base_pos )
Pipeline += [finalresult_img]
#PlotComparisonView(lane_pixel_img, finalresult_img, 'Final perspective', colormap = (None, None))
############ Save Pipeline stages results (just for the writeup ) ################
# save result into output_images folder
#for i in range( len(Pipeline)):
# if 1<= i <=3:
# cv2.imwrite("output_images/finalResultImgs"+str(i+61)+".jpg", 255*Pipeline[i])
# else:
# cv2.imwrite("output_images/finalResultImgs"+str(i+61)+".jpg", cv2.cvtColor(Pipeline[i], cv2.COLOR_RGB2BGR))
return finalresult_img
def process_image(image):
# NOTE: The output you return should be a color image (3 channel) for processing video below
# TODO: put your pipeline here,
result = advancedLaneFindingPipeline_vid(image)
# you should return the final output (image where lines are drawn on lanes)
return result
video_output = 'output_videos/project_video_final2.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
clip1 = VideoFileClip("project_video.mp4").subclip(0, 2)
#clip1 = VideoFileClip("project_video.mp4")
left_line = Line()
right_line = Line()
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(video_output))